#include <Servo.h>

/****************************/
/* servo parts              */
/****************************/
#define I_HEAD_NECK1 0
#define I_HEAD_NECK2 1
#define I_ARM_SHOULDER_LEFT 2
#define I_ARM_BICEP_LEFT 3
#define I_ARM_ELBOW_LEFT 4
#define I_ARM_WRIST_LEFT 5
//#define I_ARM_FINGER_LEFT 6
#define I_ARM_SHOULDER_RIGHT 7
#define I_ARM_BICEP_RIGHT 8
#define I_ARM_ELBOW_RIGHT 9
#define I_ARM_WRIST_RIGHT 10
//#define I_ARM_FINGER_RIGHT 11
#define I_LEG_HIP1_LEFT 12
#define I_LEG_HIP2_LEFT 13
#define I_LEG_THIGH_LEFT 14
#define I_LEG_KNEE_LEFT 15
#define I_LEG_ANKLE_LEFT 16
#define I_LEG_HIP1_RIGHT 17
#define I_LEG_HIP2_RIGHT 18
#define I_LEG_THIGH_RIGHT 19
#define I_LEG_KNEE_RIGHT 20
#define I_LEG_ANKLE_RIGHT 21

#define N_SERVOS 20
Servo servos[N_SERVOS];
int servo_ind[N_SERVOS] = {
  I_HEAD_NECK1, I_HEAD_NECK2,
  I_ARM_SHOULDER_LEFT, I_ARM_BICEP_LEFT, I_ARM_ELBOW_LEFT, I_ARM_WRIST_LEFT,
  I_ARM_SHOULDER_RIGHT, I_ARM_BICEP_RIGHT, I_ARM_ELBOW_RIGHT, I_ARM_WRIST_RIGHT,
  I_LEG_HIP1_LEFT, I_LEG_HIP2_LEFT, I_LEG_THIGH_LEFT, I_LEG_KNEE_LEFT, I_LEG_ANKLE_LEFT,
  I_LEG_HIP1_RIGHT, I_LEG_HIP2_RIGHT, I_LEG_THIGH_RIGHT, I_LEG_KNEE_RIGHT, I_LEG_ANKLE_RIGHT
};
int servo_pin[N_SERVOS] = {
  26, 27,
  46, 47, 48, 49,
  33, 34, 35, 36,
  41, 42, 43, 44, 45,
  28, 29, 30, 31, 32
};
int servo_pos[N_SERVOS] = {
   90,  90,
   30,  90,  90,  90,
  150,  90,  90,  90,
   90,  90,  90,  90,  90,
   90,  90,  90,  90,  90
};
int servo_cal[N_SERVOS] = {
    0,   0,
   -5,  0,  0,  0,
    2,  0,  0,  0,
    4,  0,  0, -3,  2,
    1,  0,  0, 10, -2
};

boolean g_isReady = false;
unsigned long g_time=0;
void setup()
{
  Serial.begin(9600);
  Serial1.begin(9600);

  Serial.println("Initialize servomotors...");

  init_servos();
  update_servos();
  delay(5000);

  Serial.println("Wait bluetooth commands...");
  g_isReady = true;
  
  g_time = millis();
}

void loop()
{
  unsigned long cur_time = millis();
  unsigned long slicetime = cur_time-g_time;
  g_time = cur_time;

  // got bluetooth command ?
  check_commands();
  
  // update servo positions
  update_servos();
  
  // send tick 
  send_tick(slicetime);
}

